#include "link.h"
#include <iostream>
#include <Eigen/Dense>

Link::Link(Vector3f p, float t, float l){
	pos = p;
	theta = t;
	length = l;
	origin = Vector3f(0, 0, 0);
	r = (float) rand()/RAND_MAX;
	g = (float) rand()/RAND_MAX;
	b = (float) rand()/RAND_MAX;
}
Link::Link(Vector3f s, Vector3f e){
	pos = s;
	end = e;
	origin = Vector3f(0, 0, 0);
	r = (float) rand()/RAND_MAX;
	g = (float) rand()/RAND_MAX;
	b = (float) rand()/RAND_MAX;
}

Vector3f Link::getLocal(Vector3f m){
	m = m - origin;
	Vector4f a(m(0), m(1), m(2), 1);
	Matrix<float, 4, 4> t_inverse = tMatrix.inverse();

	a = t_inverse * a;

	return Vector3f(a(0), a(1), a(2));
}

Link::Link(Vector3f p, Vector3f rotate_axis, float t, float l){
	pos = p;
	theta = t;
	length = l;
	axis = rotate_axis;
	origin = Vector3f(0, 0, 0);
	r = (float) rand()/RAND_MAX;
	g = (float) rand()/RAND_MAX;
	b = (float) rand()/RAND_MAX;
}
Link::~Link(){
	for(int i=0; i< (int) vertices.size(); i++){
		if(vertices[i]!=NULL)
			delete vertices[i];
	}
}
Vector3f Link::getMid(){
	return pos + 0.5*end+ origin;
}
void Link::draw(){
	glColor3f(0,0,1);
	drawCube(origin(0) + pos(0), origin(1) + pos(1), origin(2) + pos(2));
	drawCube(origin(0) + end(0), origin(1) + end(1), origin(2) + end(2));
	glColor3f(1, 1, 1);

	glBegin(GL_LINES);
	glVertex3f(origin(0) + pos(0), origin(1) + pos(1), origin(2) + pos(2));
	glVertex3f(origin(0) + end(0), origin(1) + end(1), origin(2) +end(2));
	glEnd();
}

void Link::drawCube(float x , float y , float z){
	 glBegin(GL_QUADS);		// Draw The Cube Using quads
 
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Top Right Of The Quad (Top)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Top Left Of The Quad (Top)
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Top)
    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Top)

    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Top Right Of The Quad (Bottom)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Top Left Of The Quad (Bottom)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Bottom)
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Bottom)

    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Top Right Of The Quad (Front)
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Top Left Of The Quad (Front)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Front)
    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Front)
 
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Top Right Of The Quad (Back)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Top Left Of The Quad (Back)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Back)
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Back)
 
    glVertex3f( x - 0.02f, y + 0.02f, z + 0.02f);	// Top Right Of The Quad (Left)
    glVertex3f( x - 0.02f, y + 0.02f, z - 0.02f);	// Top Left Of The Quad (Left)
    glVertex3f( x - 0.02f, y - 0.02f, z - 0.02f);	// Bottom Left Of The Quad (Left)
    glVertex3f( x - 0.02f, y - 0.02f, z + 0.02f);	// Bottom Right Of The Quad (Left)
 
    glVertex3f( x + 0.02f, y + 0.02f, z - 0.02f);	// Top Right Of The Quad (Right)
    glVertex3f( x + 0.02f, y + 0.02f, z + 0.02f);	// Top Left Of The Quad (Right)
    glVertex3f( x + 0.02f, y - 0.02f, z + 0.02f);	// Bottom Left Of The Quad (Right)
    glVertex3f( x + 0.02f, y - 0.02f, z - 0.02f);	// Bottom Right Of The Quad (Right)
    glEnd();			// End Drawing The Cube
}
